#!/usr/bin/python3
# coding=utf8
import sys
import os
import time
import re
import json
import logging
import threading

# 继续使用系统库中的TTS和ASR模块
import hiwonder.TTS as TTS
import hiwonder.ASR as ASR
import hiwonder.ros_robot_controller_sdk as rrc
from hiwonder.Controller import Controller

# 改为使用本地模块
import ActionGroupControl as AGC
import yaml_handle

'''
    程序功能：自然语言处理控制TonyPi (Program Function: Natural Language Processing Control of TonyPi)
    运行效果： 靠近语音识别模块的麦克风，先说唤醒词"开始"。当模块上的 STA 指示灯变为蓝色
             常亮时，可以用自然语言说出指令，如"向前走三步"、"向左转"、"挥手"等。
             系统会识别指令中的意图和参数，执行相应动作，并通过语音反馈响应用户指令。
'''

# 设置日志配置
logging.basicConfig(
    level=logging.DEBUG, 
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
    handlers=[
        logging.StreamHandler(),
        logging.FileHandler('/home/pi/Desktop/BS/voice_control.log')
    ]
)
logger = logging.getLogger('voice_control')

# 从当前目录导入ActionGroupDict
from ActionGroupDict import action_group_dict

# 初始化机器人底层驱动
try:
    board = rrc.Board()
    ctl = Controller(board)
    logger.info("成功初始化机器人控制器")
except Exception as e:
    logger.error(f"初始化机器人控制器失败: {e}")

# 获取舵机配置数据
try:
    servo_data = yaml_handle.get_yaml_data(yaml_handle.servo_file_path)
    logger.info("成功加载舵机配置")
except Exception as e:
    logger.error(f"加载舵机配置失败: {e}")
    servo_data = {'servo2': 1500}  # 默认值

# 定义指令映射字典
command_map = {
    # 动作类指令 (Action commands)
    "stand": ["站立", "站好", "立正", "站直", "站着"],
    "go_forward": ["前进", "向前", "向前走", "往前走", "前行"],
    "back_fast": ["后退", "向后", "向后走", "往后走", "后移"],
    "left_move_fast": ["左移", "向左移", "向左", "往左走", "左走"],
    "right_move_fast": ["右移", "向右移", "向右", "往右走", "右走"],
    "turn_left": ["左转", "向左转", "往左转", "左拐"],
    "turn_right": ["右转", "向右转", "往右转", "右拐"],
    "wave": ["挥手", "打招呼", "挥一挥", "招手"],
    "bow": ["鞠躬", "弯腰", "行礼"],
    "squat": ["下蹲", "蹲下", "蹲一下"],
    "chest": ["庆祝", "欢呼", "胜利"],
    "wing_chun": ["永春", "练武", "功夫"],
    "left_uppercut": ["左勾拳", "左拳"],
    "right_uppercut": ["右勾拳", "右拳"],
    "left_kick": ["左踢", "左脚踢"],
    "right_kick": ["右踢", "右脚踢"],
    "twist": ["扭腰", "扭一扭", "扭动"],
    "stepping": ["踏步", "原地踏步", "走步"],
    "weightlifting": ["举重", "抬重物"]
}

# 反向映射，用于查找指令
command_lookup = {}
for action, phrases in command_map.items():
    for phrase in phrases:
        command_lookup[phrase] = action

# 参数提取正则表达式
step_pattern = re.compile(r'(\d+)[步|次|下]')
speed_pattern = re.compile(r'(快速|慢速|正常速度)')

class VoiceController:
    def __init__(self):
        self.asr = None
        self.tts = None
        self.is_running = False
        self.action_running = False
        
        # 初始化语音识别和合成模块
        try:
            # 重置变量
            self.asr = ASR.ASR()
            self.tts = TTS.TTS()
            
            # 设置唤醒词和指令词
            logger.info("正在配置语音识别模块...")
            
            # 重置模块，确保干净的状态
            self.asr.eraseWords()
            time.sleep(0.5)  # 给模块一些处理时间
            
            # 先以模式1进行初始化，然后再切换到模式2
            # 模式1是自由识别模式，模式2是命令词模式
            self.asr.setMode(1)  
            time.sleep(0.5)  # 给模块一些处理时间
            self.asr.setMode(2)  # 命令词模式
            time.sleep(0.5)  # 给模块一些处理时间
            
            logger.info("语音模块模式设置完成")
            
            # 添加唤醒词 "开始"
            self.asr.addWords(1, 'kai shi')
            logger.info("已添加唤醒词: 开始 (kai shi)")
            time.sleep(0.5)  # 给模块一些处理时间
            
            # 创建命令ID和文本的映射字典，用于后面查询结果
            self.command_id_map = {}
            
            # 添加所有可能的指令词
            index = 2
            added_commands = []
            for action, phrases in command_map.items():
                for phrase in phrases:
                    pinyin = self._text_to_pinyin(phrase)
                    if pinyin:
                        try:
                            # 添加词条到ASR模块
                            self.asr.addWords(index, pinyin)
                            # 记录ID与命令的对应关系
                            self.command_id_map[index] = phrase
                            added_commands.append(f"{phrase}({pinyin})")
                            index += 1  # 注意这里修改了索引递增的位置
                            time.sleep(0.01)  # 添加短暂延时，避免指令添加太快
                        except Exception as e:
                            logger.warning(f"添加指令词 '{phrase}' 失败: {e}")
            
            logger.info(f"已添加 {len(added_commands)} 个指令词")
            time.sleep(1.0)  # 给模块足够的处理时间
            
            # 显式激活模块，确保STA指示灯亮起
            logger.info("正在激活语音模块...")
            self.asr.setMode(2)  # 再次确认处于命令词模式
            time.sleep(0.5)
            
            # 检查模块是否正常工作
            try:
                result = self.asr.getResult()
                logger.info(f"语音模块测试结果: {result}")
            except Exception as e:
                logger.warning(f"语音模块测试失败: {e}")
            
            # 初始化舵机（不包含头部动作）
            ctl.set_pwm_servo_pulse(2, servo_data['servo2'], 500)  # 避免头部动作
            
            # 执行站立动作
            AGC.runActionGroup('stand')
            logger.info("机器人已执行站立动作")
            
            # 播放准备就绪提示
            self.tts.TTSModuleSpeak('[h0][v10][m3]', '语音控制系统已准备就绪')
            logger.info("语音控制系统已准备就绪")
            logger.info("请说唤醒词\"开始\"，然后说出您的指令")
            logger.info("例如：\"向前走\"，\"向左转\"，\"挥手\"")
            
            self.is_running = True
        except Exception as e:
            logger.error(f'初始化错误: {e}', exc_info=True)
    
    def _text_to_pinyin(self, text):
        """简单的中文转拼音映射"""
        # 这里使用简单映射，实际应用可以使用更完整的拼音库
        pinyin_map = {
            '站立': 'zhan li', '前进': 'qian jin', '后退': 'hou tui',
            '左移': 'zuo yi', '右移': 'you yi', '向左': 'xiang zuo',
            '向右': 'xiang you', '向前': 'xiang qian', '向后': 'xiang hou',
            '挥手': 'hui shou', '鞠躬': 'ju gong', '下蹲': 'xia dun',
            '左转': 'zuo zhuan', '右转': 'you zhuan', '开始': 'kai shi',
            '站好': 'zhan hao', '站直': 'zhan zhi', '站着': 'zhan zhe',
            '往前走': 'wang qian zou', '前行': 'qian xing',
            '往后走': 'wang hou zou', '后移': 'hou yi',
            '往左走': 'wang zuo zou', '左走': 'zuo zou',
            '往右走': 'wang you zou', '右走': 'you zou',
            '左拐': 'zuo guai', '右拐': 'you guai',
            '打招呼': 'da zhao hu', '挥一挥': 'hui yi hui', '招手': 'zhao shou',
            '弯腰': 'wan yao', '行礼': 'xing li',
            '蹲下': 'dun xia', '蹲一下': 'dun yi xia',
            '庆祝': 'qing zhu', '欢呼': 'huan hu', '胜利': 'sheng li',
            '永春': 'yong chun', '练武': 'lian wu', '功夫': 'gong fu',
            '左勾拳': 'zuo gou quan', '左拳': 'zuo quan',
            '右勾拳': 'you gou quan', '右拳': 'you quan',
            '左踢': 'zuo ti', '左脚踢': 'zuo jiao ti',
            '右踢': 'you ti', '右脚踢': 'you jiao ti',
            '扭腰': 'niu yao', '扭一扭': 'niu yi niu', '扭动': 'niu dong',
            '踏步': 'ta bu', '原地踏步': 'yuan di ta bu', '走步': 'zou bu',
            '举重': 'ju zhong', '抬重物': 'tai zhong wu',
        }
        return pinyin_map.get(text, '')
    
    def _extract_parameters(self, command):
        """从命令中提取参数"""
        params = {'steps': 1, 'speed': 1}  # 默认值
        
        # 提取步数
        step_match = step_pattern.search(command)
        if step_match:
            params['steps'] = int(step_match.group(1))
            logger.debug(f"提取步数参数: {params['steps']}")
            
        # 提取速度
        speed_match = speed_pattern.search(command)
        if speed_match:
            speed_text = speed_match.group(1)
            if speed_text == '快速':
                params['speed'] = 2
            elif speed_text == '慢速':
                params['speed'] = 0
            logger.debug(f"提取速度参数: {speed_text}({params['speed']})")
        
        return params
    
    def _process_command(self, command_text):
        """处理命令文本"""
        logger.info(f"处理命令: {command_text}")
        
        # 查找命令中包含的指令词
        action = None
        for keyword in command_lookup:
            if keyword in command_text:
                action = command_lookup[keyword]
                logger.info(f"找到匹配指令词: {keyword} -> {action}")
                break
        
        if action:
            # 提取参数
            params = self._extract_parameters(command_text)
            
            # 执行动作
            logger.info(f"执行动作: {action}, 参数: {params}")
            self.tts.TTSModuleSpeak('', '收到')
            
            # 根据参数重复执行动作
            self.action_running = True
            try:
                for i in range(params['steps']):
                    logger.debug(f"执行第 {i+1}/{params['steps']} 次动作")
                    AGC.runActionGroup(action, params['speed'], True)
                    time.sleep(0.5)
                logger.info(f"动作 {action} 执行完成")
            except Exception as e:
                logger.error(f"执行动作时发生错误: {e}", exc_info=True)
            finally:
                self.action_running = False
            
            return True
        else:
            # 未识别到有效指令
            self.tts.TTSModuleSpeak('', '对不起，我没有听懂')
            logger.warning("未识别到有效指令")
            return False
    
    def run(self):
        """主运行循环"""
        logger.info("开始运行语音控制系统...")
        try:
            while self.is_running:
                # 获取语音识别结果
                try:
                    data = self.asr.getResult()
                    if data:
                        logger.info(f'识别结果 ID: {data}')
                        
                        if data == 1:  # 唤醒词
                            self.tts.TTSModuleSpeak('', '我在')
                            logger.info("唤醒词触发成功，等待指令...")
                        else:
                            # 使用ID映射查找对应的命令文本
                            try:
                                # 查找ID对应的命令文本
                                if data in self.command_id_map:
                                    command_text = self.command_id_map[data]
                                    logger.info(f"识别到命令: {command_text} (ID: {data})")
                                    self._process_command(command_text)
                                else:
                                    # 如果没有找到映射，使用预定义指令
                                    logger.debug(f"ID映射未找到，使用预定义指令 ID: {data}")
                                    try:
                                        action_index = data - 2
                                        action_keys = list(command_map.keys())
                                        if 0 <= action_index < len(action_keys):
                                            action = action_keys[action_index]
                                            logger.info(f"使用预定义动作: {action}")
                                            self.tts.TTSModuleSpeak('', '收到')
                                            AGC.runActionGroup(action, 1, True)
                                        else:
                                            logger.warning(f"无效的动作索引: {action_index}")
                                            self.tts.TTSModuleSpeak('', '对不起，我没有听懂')
                                    except Exception as e:
                                        logger.error(f"执行预定义动作时出错: {e}", exc_info=True)
                                        self.tts.TTSModuleSpeak('', '对不起，我没有听懂')
                            except Exception as e:
                                logger.error(f"处理语音识别结果时出错: {e}", exc_info=True)
                except Exception as e:
                    logger.error(f"获取语音识别结果时出错: {e}")
                    # 如果获取结果出错，尝试重新初始化模块
                    try:
                        logger.info("尝试重置语音模块...")
                        self.asr.setMode(2)  # 重新设置为命令词模式
                        time.sleep(1.0)
                    except:
                        pass
                    
                time.sleep(0.01)
                
        except KeyboardInterrupt:
            logger.info("程序已被用户停止")
            self.is_running = False
        except Exception as e:
            logger.error(f"运行时发生错误: {e}", exc_info=True)
            self.is_running = False

# 主程序入口点
if __name__ == "__main__":
    logger.info("===== 语音控制程序启动 =====")
    try:
        controller = VoiceController()
        controller.run()
    except Exception as e:
        logger.critical(f"程序发生致命错误: {e}", exc_info=True)
    finally:
        logger.info("===== 语音控制程序结束 =====")